/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.Joystick;

/**
 *
 * @author Driver
 */
public class Utils {
/**
     * Calculate the angle (in degrees) of the right triangle formed with legs
     * x and y
     * @param x horizontal leg (adjacent)
     * @param y vertical leg (opposite)
     * @return Angle (in degrees) formed
     */
    public static double calculateAng(double x, double y)
    {
        return (MathUtils.atan2(y, x)* 180)/Constants.PI;
    }

    public static double mod360(double angle)
    {
        while(angle < 0)
        {
            angle += 360;
        }

        return angle % 360;
    }
    
    public static double limitValue( double value, double valueLimit )
    {
        if ( value > valueLimit )
        {
            value = valueLimit;
        }
        else if ( value < -valueLimit )
        {
            value = -valueLimit;
        }
        return value;
    }
    
//    public static String fmt6_3( double x)
//    {
//        return String.format("f6.3", x);
//    }
    
       /**
    * Format63f()
    *
    * The JVM in the cRIO does not support String.format()
    * It also doesn't support Math.round()
    * This method will format with specifier %6.3f
    * We use this to print speeds
    */
    public static String fmt6_3(double dValue){

        String s = "";

        //long iValue = Math.abs(Math.round(dValue*1000.0));
        double dTemp = dValue*1000.0;
        long iValue = (long)Math.abs( dTemp > 0.0 ? dTemp + 0.5 : dTemp - 0.5 );

        long d1;        // integer digit(s)
        long d2;        // 1st decimal digit
        long d3;        // 2nd decimal digit
        long d4;        // 3rd decimal digit

        d1 = iValue / 1000;
        d2 = ( iValue - d1*1000 ) / 100;
        d3 = ( iValue - d1*1000 - d2*100 ) / 10;
        d4 = ( iValue - d1*1000 - d2*100 - d3*10 );


        if( dValue < 0.0 )
            s = s + "-";
        else
            s = s + " ";

        s = s + d1;

        s = s + ".";

        s = s + d2;
        s = s + d3;
        s = s + d4;

        return s;
    }
    
    /*
     * Gets lowest button being pressed on a joystick
     */
    public static int getLowestButton(Joystick joy){
        for(int i=1;i<=12;i++){
            if(joy.getRawButton(i)){
                return i;
            }
        }
        return 0;
    }
    
    /*
     * Returns the magnitude of the joystick given its X and Y vectors
     */
    public static double getJoyMagnitude(double x, double y){
        //return Math.sqrt((x*x)+(y*y));
        return Math.max(Math.abs(x),Math.abs(y));
    }
}
